#include "TaskStat.h"

namespace common {
namespace statistics{
  /**
   * @brief 数据初始化
   * @param bound_type 任务外框类型
   * @param boundary 任务外框（世界坐标系）
   * @param clean_paths 任务路径（世界坐标系）
   * @param clean_width 清扫宽度, unit: m
   * @param origin 栅格地图左下角栅格的左下角点对应的世界坐标
   * @param resolution 栅格地图分辨率, unit: m
   * @param width 栅格地图的宽, unit: 栅格
   * @param height 栅格地图的高, unit: 栅格
   */
  void TaskStat::Init(const uint8_t bound_type,
                       const PoseVec& boundary,
                       const std::vector<PoseVec>& clean_paths,
                       const double clean_width,
                       const Pose& origin,
                       const double resolution,
                       const int64_t width,
                       const int64_t height) {
      this->bound_type = bound_type;
      this->boundary = boundary;
      this->clean_paths = clean_paths;
      this->clean_width = clean_width;
      this->cv_map.SetOrigin(origin);
      this->cv_map.SetResolution(resolution);
      this->cv_map.MapData() = cv::Mat::zeros(height, width, CV_8UC1);
    }

    /**
     * @brief 根据任务外框、任务路径、清扫宽度计算待清洁的栅格, 保存至cv_map的非0栅格中
     * @param morph_close 是否对待清洁区做形态学闭操作 (去除狭小黑色区域)
     * @param close_kenel_size 形态学闭操作核的尺寸
     */
    void TaskStat::CalcGridsToClean(const bool morph_close,
                                    const int close_kenel_size) {
      // 1. 把清扫路线覆盖的区域涂白
      int line_width = (int)((clean_width + 1e-3) / cv_map.Resolution());
      LOG(INFO) << "line_width is " << line_width;
      for (const auto& clean_path : clean_paths) {
        std::vector<cv::Point> cv_clean_path;
        for (const auto& point : clean_path) {
          cv_clean_path.emplace_back(
              cv::Point(cv_map.X2IX(point.X()), cv_map.Y2IY(point.Y())));
        }
        for (size_t i = 0; i + 1 < cv_clean_path.size(); ++i) {
          cv::line(cv_map.MapData(), cv_clean_path.at(i),
                   cv_clean_path.at(i + 1), cv::Scalar(255), line_width);
        }
      }
      if (bound_type == NO_BOUND ||
          boundary.size() < 3) {  // 贴边/示教任务 || 非法外框
        return;
      }

      // 区域清扫任务

      // 2. 任务框内的区域涂白
      cv::Mat mask_map =
          cv::Mat::zeros(cv_map.Height(), cv_map.Width(), CV_8UC1);
      std::vector<cv::Point> cv_boundary;
      for (const auto& point : boundary) {
        cv_boundary.emplace_back(
            cv::Point(cv_map.X2IX(point.X()), cv_map.Y2IY(point.Y())));
      }
      cv::drawContours(mask_map,
                       std::vector<std::vector<cv::Point>>(1, cv_boundary), -1,
                       cv::Scalar(255),
                       cv::FILLED);  // 注: 外框多边形存在自交叉的情况,
                                     // 类似"8"字的没有问题 !!!
      if (bound_type == TEACH_BOUND) {  // 示教任务框
        cv::drawContours(mask_map,
                         std::vector<std::vector<cv::Point>>(1, cv_boundary),
                         -1, cv::Scalar(255), line_width);
      }

      // 3. 前两者取与, 完成所有待清洁区的标记
      cv::bitwise_and(cv_map.MapData(), mask_map, cv_map.MapData());

      // 4. 形态学闭操作 -> 去除狭小黑色区域
      if (morph_close) {
        cv::Mat kernel = cv::getStructuringElement(
            cv::MORPH_ELLIPSE, cv::Size(close_kenel_size, close_kenel_size),
            cv::Point(-1, -1));
        cv::morphologyEx(cv_map.MapData(), cv_map.MapData(), cv::MORPH_CLOSE,
                        kernel);
      }
    }

} //end of namespace statistics
} //end of namespace common